#include "C_H.h"
// #include "kalman.h"
//  һ�׹����˲���

lpf_controller lpf_controller_l, lpf_controller_r;

lpf_controller lpf_load;

kalman_controller R_encoderkal, L_encoderkal;
// float R_Q = 0.03, R_R = 0.4;
// float L_Q = 0.03, L_R = 0.4;
float R_Q = 0.03, R_R = 0.4;  // 更小的 Q，更平滑
float L_Q = 0.03, L_R = 0.4;

void lpf_init(lpf_controller *lpf)
{
    lpf->last_out = 0;
    return;
}

int lpf_update(lpf_controller *lpf, int input)
{
    return lpf->last_out = lpf->alpha * input + (1 - lpf->alpha) * lpf->last_out;
}

// �������˲���KF

kalman_controller kalman_controller_q, kalman_controller_h, kalman_controller_d3, kalman_controller_d4;

kalman_controller kalman_err_q, kalman_err_h, kalman_err_d, kalman_sensor[7];

float kalman_update(kalman_controller *klm, float input)
{
    // Ԥ��Э����̣�kʱ��ϵͳ����Э���� = k-1ʱ�̵�ϵͳЭ���� + ��������Э����
    klm->Now_P = klm->LastP + klm->Q;
    // ���������淽�̣����������� = kʱ��ϵͳ����Э���� / ��kʱ��ϵͳ����Э���� + �۲�����Э���
    klm->Kg = klm->Now_P / (klm->Now_P + klm->R);
    // ��������ֵ���̣�kʱ��״̬����������ֵ = ״̬������Ԥ��ֵ + ���������� * ������ֵ - ״̬������Ԥ��ֵ��
    klm->out = klm->out + klm->Kg * (input - klm->out); // ��Ϊ��һ�ε�Ԥ��ֵ������һ�ε����ֵ
    // ����Э�����: ���ε�ϵͳЭ����� klm->LastP Ϊ��һ������׼����
    klm->LastP = (1 - klm->Kg) * klm->Now_P;
    return (klm->out);
}

float q_Q = 0.03, q_R = 0.4;
float h_Q = 0.03, h_R = 0.4;
float d_Q = 0.1, d_R = 4.0;
void kalman_init(kalman_controller *klm, float klm_Q, float klm_R) // �¶�klm_Q=0.01 klm_R=0.25
{
    klm->LastP = 1.03; // �ϴι���Э����  0.5
    klm->Now_P = 0;    // ��ǰ����Э����
    klm->out = 0;      // �������˲������
    klm->Kg = 0;       // ����������
    klm->Q = klm_Q;    // Q:��������Э���� Q�������˲��������ƽ���̶ȣ�QԽСԽƽ��;Q��������
    klm->R = klm_R;    // R:�۲�����Э���� R���������˲����������ʵ�����ߵ�����̶ȣ�RԽСԽ�ӽ�(����Խ��),RԽ���������,R��������
}
